#ifndef EPUCK_DRIVER_ROS
#define EPUCK_DRIVER_ROS

#include "ros/ros.h"
#include "nav_msgs/Odometry.h"
#include "geometry_msgs/Pose2D.h"
#include "geometry_msgs/Twist.h"
#include "std_msgs/Bool.h"

#include "epuckDriver/serialPort.hpp"
#include "epuckDriver/epuckPosition2d.hpp"
#include "epuckDriver/epuckIR.hpp"
#include "epuckDriver/epuckLEDs.hpp"
#include "epuckDriver/epuckCamera.hpp"

#include "epuckDriver/SetOdometry.h"
#include "epuckDriver/ResetOdometry.h"
#include "epuckDriver/GetGeometry.h"
#include "epuckDriver/GetIRPose2D.h"
#include "epuckDriver/GetOdometry.h"

#include "epuckDriver/OdometryPublisherMsg.h"
#include "epuckDriver/CommandSubscriberMsg.h"
#include "epuckDriver/IRDataPublisherMsg.h"
#include "epuckDriver/CameraDataMsg.h"
#include "epuckDriver/BoolMsg.h"

#include <boost/thread/mutex.hpp>
#include <string>
#include <memory>

class EpuckDriver{

public:

    EpuckDriver(std::string iD,std::string port);
    ~EpuckDriver();

    int Start(); 
    void Publish(bool vel,bool ir, bool cam);

    bool ResetOdometry(epuckDriver::ResetOdometry::Request& req,
epuckDriver::ResetOdometry::Response& res);
    bool SetOdometry(epuckDriver::SetOdometry::Request& req, 
epuckDriver::SetOdometry::Response &res);
    
    bool GetGeometry(epuckDriver::GetGeometry::Request& req,
epuckDriver::GetGeometry::Response &res); 
    bool GetIRPose2D(epuckDriver::GetIRPose2D::Request& req,
epuckDriver::GetIRPose2D::Response &res); 
    bool GetOdometry(epuckDriver::GetOdometry::Request& req,
epuckDriver::GetOdometry::Response &res);  

    const char* WhereServerSetOdom(){
          return epuckServerSetOdom.c_str(); 
    }

    const char* WhereServerResetOdom(){
          return epuckServerResetOdom.c_str(); 
    }

    const char* WhereServerGetGeom(){
          return epuckServerGetGeom.c_str(); 
    }
    
    const char* WhereServerGetOdom(){
          return epuckServerGetOdom.c_str(); 
    }

    const char* WhereServerGetIRPose(){
          return epuckServerGetIRPose.c_str(); 
    }

    const char* WherePubOdometry(){
          return epuckPubOdometry.c_str(); 
    }

    const char* WherePubIR(){
          return epuckPubIR.c_str(); 
    }

    const char* WherePubCamera(){
          return epuckPubCamera.c_str(); 
    }

    const char* WhereSubVelocity(){
          return epuckSubVelocity.c_str(); 
    }

    const char* WhereSubCommand(){
          return epuckSubCommand.c_str(); 
    }

    const char* WhereSubFrontLED(){
          return epuckSubFrontLED.c_str(); 
    }

    const char* WhereSubBodyLED(){
          return epuckSubBodyLED.c_str(); 
    }

    const char* WhereSubRingLED(){
          return epuckSubRingLED.c_str(); 
    }
 
/*----------------POSITION2D---------------------------------*/
    void VelocityCallBack(const geometry_msgs::Twist::ConstPtr& twist);
    void CommandCallBack(const epuckDriver::CommandSubscriberMsg::ConstPtr& commandSubscriberMsg);
/*--------------------------------------------------------*/
    
/*---------------------LED---------------------------------*/
    void FrontLEDCallBack(const std_msgs::Bool::ConstPtr& front);
    void BodyLEDCallBack(const std_msgs::Bool::ConstPtr& body);
    void RingLEDCallBack(const epuckDriver::BoolMsg::ConstPtr& ring);
/*--------------------------------------------------------*/
    
private:
      
    int Setup();       
    void Init();
 
/*----------------POSITION2D---------------------------------*/
    void OdometryPublisher();
    
    ros::Publisher  pubOdometry;

    ros::ServiceServer setOdometry;
    ros::ServiceServer resetOdometry;
    ros::ServiceServer getGeometry;
    
    geometry_msgs::Twist twist;
    geometry_msgs::Pose2D pose2d;    
 
    std::auto_ptr<EpuckPosition2d> epuckPosition2d;
/*--------------------------------------------------------*/
    
/*---------------------IR---------------------------------*/
    void IRPublisher();

    ros::Publisher pubIR;
       
    ros::ServiceServer getIRPose2d;
 
    epuckDriver::IRDataPublisherMsg irData;   

    std::auto_ptr<EpuckIR> epuckIR;
/*--------------------------------------------------------*/

/*----------------------CAMERA----------------------------*/
    void CameraPublisher();    

    ros::Publisher pubCamera;

    epuckDriver::CameraDataMsg cameraData;    

    std::auto_ptr<EpuckCamera> epuckCamera;
/*--------------------------------------------------------*/

/*---------------------LED---------------------------------*/
    std::auto_ptr<EpuckLEDs> epuckLEDs;
/*--------------------------------------------------------*/

    SerialPort *serialPort;
    ros::NodeHandle n_epuck;
    
    const unsigned EXPECTED_EPUCK_SIDE_VERSION;    
    
    bool exec;

    int iD;

    std::string epuckPubOdometry;
    std::string epuckPubIR; 
    std::string epuckPubCamera;
    
    std::string epuckSubVelocity;
    std::string epuckSubCommand;
    std::string epuckSubFrontLED;
    std::string epuckSubBodyLED;
    std::string epuckSubRingLED;

    std::string epuckServerSetOdom;
    std::string epuckServerResetOdom; 
    std::string epuckServerGetGeom;
    std::string epuckServerGetIRPose;
    std::string epuckServerGetOdom;

    boost::mutex epuckSignal;
};

#endif
